﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace Simulador_Quadrotor
{
    //clase controlador PID para cuadrotor
    //Solo proporcional por ahora
    public class PID
    {
        #region Propiedades

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Kp { get; set; }

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Kd { get; set; }

        /// <summary>
        /// constante de proporcionalidad
        /// </summary>
        public double Ki { get; set; }

        /// <summary>
        /// Estados anteriores
        /// </summary>
        public double errorAnterior { get; set; }

        public double errorI { get; set; }

        /// <summary>
        /// Tamaño del paso
        /// </summary>
        public double Tp { get; set; }

        #endregion


        #region Constructoras

        public PID()
        {
            this.errorAnterior = 0;
            this.errorI = 0;
        }

        #endregion

        /// <summary>
        /// Control PID
        /// </summary>
        /// <param name="x">estado actual</param>
        /// <param name="xd">estado deseado</param>
        /// <param name="xd">estado de los rotores anterior</param>

        public double Control(Estado x, Estado xd, double errorP)
        {

            double errorD = 0;
            //error integrativo

            errorI = errorI + (errorP * this.Tp);

            //error derivativo

            errorD = (errorP - this.errorAnterior) / this.Tp;

            double P = errorP * Kp;
            double D = errorD * Kd;
            double I = errorI * Ki;
            double incremento = P + D + I;

            this.errorAnterior = errorP;

            return incremento;

        }
    }
}
